# pragma once

# include "LTP_A_header.h"

namespace LTP_A
{
    class anchor
    {
    private:
        const size_t fixed_part_size = 896;
        const uint8_t frame_header = 0x55;
        const uint8_t function_mark = 0x00;
        const uint8_t tail_check = 0xee;
        nlt_anchorframe0_result_t result;

        std::vector<uint8_t> mem_data;
        serialib serial;
        
        std::string device;
        int bauds;

        nlt_anchorframe0_raw_t g_frame;
        uint8_t init_needed = 1;
        uint8_t UnpackData(const uint8_t *data, size_t data_length) {
            if (data_length < fixed_part_size ||
                data[0] != frame_header ||
                data[1] != function_mark ||
                data[fixed_part_size - 1] !=
                    tail_check)
                return 0;

            if (init_needed) {
                memset(result.nodes, 0,
                    sizeof(result.nodes));
                init_needed = 0;
            }

            memcpy(&g_frame, data, fixed_part_size);
            result.role = linktrack_role_e::LINKTRACK_ROLE_TAG; // 官方源码这里有问题，我就直接赋值了
            result.id = g_frame.id;
            result.local_time = g_frame.local_time;
            result.system_time = g_frame.system_time;
            result.voltage = g_frame.voltage / MULTIPLY_VOLTAGE;

            result.valid_node_count = 0;
            for (size_t i = 0, count = ARRAY_LENGTH(result.nodes);
                i < count; ++i) {
                if (g_frame.nodes[i].id == 0xff)
                continue;

                uint8_t index = result.valid_node_count;
                TRY_MALLOC_NEW_NODE(result.nodes[index],
                                    nlt_anchorframe0_node_t)

                result.nodes[index]->role =
                    (linktrack_role_e)g_frame.nodes[i].role;
                result.nodes[index]->id = g_frame.nodes[i].id;
                NLINK_TRANSFORM_ARRAY_INT24(result.nodes[index]->pos_3d,
                                            g_frame.nodes[i].pos_3d, MULTIPLY_POS)
                NLINK_TRANSFORM_ARRAY(result.nodes[index]->dis_arr,
                                    g_frame.nodes[i].dis_arr, 100.0f)

                ++result.valid_node_count;
            }
            return 1;
        }
    public:
        char status;

        anchor(const char * arg1, int arg2)
            :device(arg1), bauds(arg2)
        {
            status = serial.openDevice(device.data(),bauds);
            if (status != 1)
            {
                throw std::runtime_error("uwb driver error: open device: " + device);
            }

            std::cout << "uwb driver online at " << device << std::endl;
        };
        ~anchor()
        {
            serial.closeDevice();
        };

        /// @brief 获取端口处的uwb数据
        /// @return 
        //      -2：端口没有打开
        //      -1: 没有检查到有效头；
        //      0：检查到有效头，但是长度不够；
        //      1：检查到有效数据帧
        int reciveData()
        {
            // std::cout << mem_data.size() << std::endl; // 有没有出现数据缓存问题就看这个

            std::vector<uint8_t> catData;
            if (mem_data.size() > 0)
            {
                catData.insert(catData.end(),mem_data.begin(),mem_data.end());
                mem_data.clear();
            }
            
            auto length = serial.available();
            if (length > 0)
            {
                std::vector<uint8_t> buffer(length);
                if (serial.isDeviceOpen())
                {
                    serial.readBytes(buffer.data(),length);
                }
                else
                {
                    std::cout << "uwb driver error:  device not open ()" << std::endl;
    
                    return -2;
                }

                catData.insert(catData.end(),buffer.begin(),buffer.end());
            }
            
            int n_bytes = catData.size();
            int n = 0;
            if (n_bytes == 0)
            {
                return -1;
            }
            while (n + 1 < n_bytes)
            {
                if (catData[n] == frame_header && catData[n + 1] == function_mark)
                {
                    if (n_bytes - n >= fixed_part_size)
                    {
                        auto r = UnpackData(catData.data() + n,n_bytes - n);

                        if (r == 0)
                        {
                            n = n + 1;
                            continue;
                        }
                        else
                        {  
                            mem_data.insert(mem_data.end(),catData.begin() + n + fixed_part_size,catData.end());

                            return 1;
                        }                    
                    }
                    else
                    {
                        mem_data.insert(mem_data.end(),catData.begin() + n,catData.end());

                        return 0;
                    }
                }
                else
                {
                    n = n + 1;
                }
            }

            // 特殊情况，数据的最后一个字节刚好就是帧头
            if (catData[n_bytes - 1] == frame_header)
            {
                mem_data.insert(mem_data.end(),catData.end(),catData.end());

                return 0;
            }
            
            return -1;
        }

        const nlt_anchorframe0_result_t *get_data()
        {
            return &result;
        }

        anchor(const anchor&) = delete;
        anchor operator= (const anchor&) = delete;
    };
} // namespace LTP_A
